/**********************************************************
**             Email:@qq.com   QQ:1069841355
**---------------------------------------------------------
**  Description: 此文件为 四足机器人 仿真环境 接口文件
**  Version    : 
**  Notes      : 
**  Author     : 于宪元
**********************************************************/
#include <stdio.h>

#include <webots/robot.h>
#include <webots/motor.h>
#include <webots/position_sensor.h>
#include <webots/touch_sensor.h>
#include <webots/keyboard.h>
#include <webots/inertial_unit.h>

#include "webotsInterface.h"

//-----------------------------------------------------------device
/*电机*/
WbDeviceTag FL0_motor;
WbDeviceTag FL1_motor;
WbDeviceTag FL2_motor;

WbDeviceTag FR0_motor;
WbDeviceTag FR1_motor;
WbDeviceTag FR2_motor;

WbDeviceTag BL0_motor;
WbDeviceTag BL1_motor;
WbDeviceTag BL2_motor;

WbDeviceTag BR0_motor;
WbDeviceTag BR1_motor;
WbDeviceTag BR2_motor;

/*电机编码器*/
WbDeviceTag FL0_pos_sensor;
WbDeviceTag FL1_pos_sensor;
WbDeviceTag FL2_pos_sensor;

WbDeviceTag FR0_pos_sensor;
WbDeviceTag FR1_pos_sensor;
WbDeviceTag FR2_pos_sensor;

WbDeviceTag BL0_pos_sensor;
WbDeviceTag BL1_pos_sensor;
WbDeviceTag BL2_pos_sensor;

WbDeviceTag BR0_pos_sensor;
WbDeviceTag BR1_pos_sensor;
WbDeviceTag BR2_pos_sensor;

/*足底触碰开关*/
WbDeviceTag FL_touch_sensor;
WbDeviceTag FR_touch_sensor;
WbDeviceTag BL_touch_sensor;
WbDeviceTag BR_touch_sensor;

/*惯导系统*/
WbDeviceTag IMU;

/*
函数功能：初始化devices
*/
void webots_device_init()
{
  //get device
  FL0_motor          = wb_robot_get_device("LFJ0Motor");
  FL1_motor          = wb_robot_get_device("LFJ1Motor");
  FL2_motor          = wb_robot_get_device("LFJ2Motor");
  
  FR0_motor          = wb_robot_get_device("RFJ0Motor");
  FR1_motor          = wb_robot_get_device("RFJ1Motor");
  FR2_motor          = wb_robot_get_device("RFJ2Motor");
  
  BL0_motor          = wb_robot_get_device("LBJ0Motor");
  BL1_motor          = wb_robot_get_device("LBJ1Motor");
  BL2_motor          = wb_robot_get_device("LBJ2Motor");
  
  BR0_motor          = wb_robot_get_device("RBJ0Motor");
  BR1_motor          = wb_robot_get_device("RBJ1Motor");
  BR2_motor          = wb_robot_get_device("RBJ2Motor");
  
  
  FL0_pos_sensor     = wb_robot_get_device("LFJ0Sensor");
  FL1_pos_sensor     = wb_robot_get_device("LFJ1Sensor");
  FL2_pos_sensor     = wb_robot_get_device("LFJ2Sensor");
  
  FR0_pos_sensor     = wb_robot_get_device("RFJ0Sensor");
  FR1_pos_sensor     = wb_robot_get_device("RFJ1Sensor");
  FR2_pos_sensor     = wb_robot_get_device("RFJ2Sensor");
  
  BL0_pos_sensor     = wb_robot_get_device("LBJ0Sensor");
  BL1_pos_sensor     = wb_robot_get_device("LBJ1Sensor");
  BL2_pos_sensor     = wb_robot_get_device("LBJ2Sensor");
  
  BR0_pos_sensor     = wb_robot_get_device("RBJ0Sensor");
  BR1_pos_sensor     = wb_robot_get_device("RBJ1Sensor");
  BR2_pos_sensor     = wb_robot_get_device("RBJ2Sensor");

  
  FL_touch_sensor    = wb_robot_get_device("LF_Touch");
  FR_touch_sensor    = wb_robot_get_device("RF_Touch");
  BL_touch_sensor    = wb_robot_get_device("RB_Touch");
  BR_touch_sensor    = wb_robot_get_device("LB_Touch");
  
  IMU                = wb_robot_get_device("inertial unit");
  
  //enable
  wb_position_sensor_enable(FL0_pos_sensor, TIME_STEP);
  wb_position_sensor_enable(FL1_pos_sensor, TIME_STEP);
  wb_position_sensor_enable(FL2_pos_sensor, TIME_STEP);

  wb_position_sensor_enable(FR0_pos_sensor, TIME_STEP);
  wb_position_sensor_enable(FR1_pos_sensor, TIME_STEP);
  wb_position_sensor_enable(FR2_pos_sensor, TIME_STEP);
  
  wb_position_sensor_enable(BL0_pos_sensor, TIME_STEP);
  wb_position_sensor_enable(BL1_pos_sensor, TIME_STEP);
  wb_position_sensor_enable(BL2_pos_sensor, TIME_STEP);
  
  wb_position_sensor_enable(BR0_pos_sensor, TIME_STEP);
  wb_position_sensor_enable(BR1_pos_sensor, TIME_STEP);
  wb_position_sensor_enable(BR2_pos_sensor, TIME_STEP);  
  
  wb_touch_sensor_enable(FL_touch_sensor, TIME_STEP);
  wb_touch_sensor_enable(FR_touch_sensor, TIME_STEP);
  wb_touch_sensor_enable(BL_touch_sensor, TIME_STEP);
  wb_touch_sensor_enable(BR_touch_sensor, TIME_STEP);
  
  wb_inertial_unit_enable(IMU, TIME_STEP);
  
  wb_keyboard_enable(TIME_STEP);
}
//-----------------------------------------------------------motor
/*
函数功能：设置电机扭矩
*/
void set_motor_torque(motorNameTypeDef motorName, double torque)
{
  if(torque >  1000)torque =  1000;
  if(torque < -1000)torque = -1000;
  switch (motorName){
  case FL0:  {  wb_motor_set_torque(FL0_motor, torque);break;  }
  case FL1:  {  wb_motor_set_torque(FL1_motor, torque);break;  }
  case FL2:  {  wb_motor_set_torque(FL2_motor, torque);break;  }
  
  case FR0:  {  wb_motor_set_torque(FR0_motor, torque);break;  }
  case FR1:  {  wb_motor_set_torque(FR1_motor, torque);break;  }
  case FR2:  {  wb_motor_set_torque(FR2_motor, torque);break;  }
  
  case BL0:  {  wb_motor_set_torque(BL0_motor, torque);break;  }
  case BL1:  {  wb_motor_set_torque(BL1_motor, torque);break;  }
  case BL2:  {  wb_motor_set_torque(BL2_motor, torque);break;  }
  
  case BR0:  {  wb_motor_set_torque(BR0_motor, torque);break;  }
  case BR1:  {  wb_motor_set_torque(BR1_motor, torque);break;  }
  case BR2:  {  wb_motor_set_torque(BR2_motor, torque);break;  }

  default:break;
  }
}
/*
函数功能：设置电机位置
*/
void set_motor_position(motorNameTypeDef motorName, double position)
{
  switch (motorName){
  case FL0:  {  wb_motor_set_position(FL0_motor, position);break;  }
  case FL1:  {  wb_motor_set_position(FL1_motor, position);break;  }
  case FL2:  {  wb_motor_set_position(FL2_motor, position);break;  }
  
  case FR0:  {  wb_motor_set_position(FR0_motor, position);break;  }
  case FR1:  {  wb_motor_set_position(FR1_motor, position);break;  }
  case FR2:  {  wb_motor_set_position(FR2_motor, position);break;  }
  
  case BL0:  {  wb_motor_set_position(BL0_motor, position);break;  }
  case BL1:  {  wb_motor_set_position(BL1_motor, position);break;  }
  case BL2:  {  wb_motor_set_position(BL2_motor, position);break;  }
  
  case BR0:  {  wb_motor_set_position(BR0_motor, position);break;  }
  case BR1:  {  wb_motor_set_position(BR1_motor, position);break;  }
  case BR2:  {  wb_motor_set_position(BR2_motor, position);break;  }

  default:break;
  }
}
//-----------------------------------------------------------sensor
/*
函数功能：获取电机角度,角度制
*/
double get_motor_angle(motorNameTypeDef motorName)
{
  double angle = 0;
  switch (motorName){
  case FL0:  { angle = wb_position_sensor_get_value(FL0_pos_sensor);break; }
  case FL1:  { angle = wb_position_sensor_get_value(FL1_pos_sensor);break; }
  case FL2:  { angle = wb_position_sensor_get_value(FL2_pos_sensor);break; }
  
  case FR0:  { angle = wb_position_sensor_get_value(FR0_pos_sensor);break; }
  case FR1:  { angle = wb_position_sensor_get_value(FR1_pos_sensor);break; }
  case FR2:  { angle = wb_position_sensor_get_value(FR2_pos_sensor);break; }
  
  case BL0:  { angle = wb_position_sensor_get_value(BL0_pos_sensor);break; }
  case BL1:  { angle = wb_position_sensor_get_value(BL1_pos_sensor);break; }
  case BL2:  { angle = wb_position_sensor_get_value(BL2_pos_sensor);break; }
  
  case BR0:  { angle = wb_position_sensor_get_value(BR0_pos_sensor);break; }
  case BR1:  { angle = wb_position_sensor_get_value(BR1_pos_sensor);break; }
  case BR2:  { angle = wb_position_sensor_get_value(BR2_pos_sensor);break; }
  default:break;
  }
  return angle*180.0f/PI;
}
/*
函数功能：检测足底是否接触地面
*/
bool is_foot_touching(legNameTypeDef legName)
{
  if(legName == FL)
    return wb_touch_sensor_get_value(FL_touch_sensor);
  if(legName == FR)
    return wb_touch_sensor_get_value(FR_touch_sensor);
  if(legName == BL)
    return wb_touch_sensor_get_value(BL_touch_sensor);
  if(legName == BR)
    return wb_touch_sensor_get_value(BR_touch_sensor);    
  return true;
}

/*
函数功能：读取IMU数据
*/
eulerAngleTypeDef get_IMU_Angle()
{
  const double* data = wb_inertial_unit_get_roll_pitch_yaw(IMU);
  
  eulerAngleTypeDef eulerAngle;
  eulerAngle.roll  = data[1]*180.0f/PI;
  eulerAngle.pitch = data[0]*180.0f/PI;
  eulerAngle.yaw   = data[2]*180.0f/PI;
  
  return eulerAngle;
}
/*
函数功能：所有电机抱闸停止运动
*/
void all_motor_stop()
{
  for(motorNameTypeDef motor = FL0; motor <=BR2; motor++)
  {
    set_motor_position(motor, get_motor_angle(motor));
  }
}
  
//-----------------------------------------------------------keyboard
/*
函数功能：读取键盘键值
*/
int get_keyboard()
{
  return wb_keyboard_get_key();
}

